//
// Created by ubuntu on 19-5-13.
//
#ifndef RELOCATION_SLAM_TRIANGLE_H
#define RELOCATION_SLAM_TRIANGLE_H
#include <iostream>
#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include "Eigen/Core"
#include "Eigen/Geometry"
#include <opencv2/core/eigen.hpp>
#include "Frame.h"
using namespace Eigen;
using namespace std;

cv::Point2f pixel2cam ( const cv::Point2d& p, const cv::Mat& K )
{
    return cv::Point2f
    (
        ( p.x - K.at<double>(0,2) ) / K.at<double>(0,0),
        ( p.y - K.at<double>(1,2) ) / K.at<double>(1,1)
    );
}

//void triangulation (
//        Relocation_Slam::Frame* f1,
//        Relocation_Slam::Frame* f2,
//        cv::Mat K,
//        const std::vector< cv::DMatch >& matches,
//        vector<int>& f1p_ids,
//        vector<int>& f2p_ids,
//        vector< cv::Point3f >& points )
//{
////    Mat T1 = (Mat_<float> (3,4) <<
////            1,0,0,0,
////            0,1,0,0,
////            0,0,1,0);
////    Mat T2 = (Mat_<float> (3,4) <<
////            R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), t.at<double>(0,0),
////            R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), t.at<double>(1,0),
////            R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), t.at<double>(2,0)
////    );
//
////    Mat K = ( Mat_<double> ( 3,3 ) << 520.9, 0, 325.1, 0, 521.0, 249.7, 0, 0, 1 );
//
//    cv:: Mat T1;
//    cv::eigen2cv(f1->mPose.matrix(),T1);
//    //std::cout<<"T1:"<<std::endl<<T1<<std::endl;
//
//    //转成3*4矩阵
//    cv::Mat frame_t1 = (cv::Mat_<double> (3,4) <<
//            T1.at<double>(0,0),T1.at<double>(0,1),T1.at<double>(0,2),T1.at<double>(0,3),
//            T1.at<double>(1,0), T1.at<double>(1,1), T1.at<double>(1,2), T1.at<double>(1,3),
//            T1.at<double>(2,0), T1.at<double>(2,1), T1.at<double>(2,2), T1.at<double>(2,3));
//
//    cv::Mat T2;
//    cv::eigen2cv(f2->mPose.matrix(),T2);
//    //std::cout<<"T2:"<<std::endl<<T2<<std::endl;
//
//    //转成3*4矩阵
//    cv::Mat frame_t2 = (cv::Mat_<double> (3,4) <<
//            T2.at<double>(0,0),T2.at<double>(0,1),T2.at<double>(0,2),T2.at<double>(0,3),
//            T2.at<double>(1,0), T2.at<double>(1,1), T2.at<double>(1,2), T2.at<double>(1,3),
//            T2.at<double>(2,0), T2.at<double>(2,1), T2.at<double>(2,2), T2.at<double>(2,3));
//
//    vector<cv::Point2f> pts_1, pts_2;
//    vector<int> keyPoints_idx;
////    for ( DMatch m:matches )
////    {
////        // 将像素坐标转换至相机坐标
////        pts_1.push_back ( pixel2cam( f1.mvKeyPoints[m.queryIdx].pt, K) );
////        pts_2.push_back ( pixel2cam( f2.mvKeyPoints[m.trainIdx].pt, K) );
////        keyPoints_idx.push_back(m.queryIdx);
////    }
//
//    for ( int i=0;i<f1p_ids.size();i++ )
//    {
//        // 将像素坐标转换至相机坐标
//        pts_1.push_back ( pixel2cam( f1->mvKeyPoints[f1p_ids[i]].pt, K) );
//        pts_2.push_back ( pixel2cam( f2->mvKeyPoints[f2p_ids[i]].pt, K) );
//    }
//
//    cv::Mat pts_4d;
//    cv::Mat fK;
//    K.convertTo(fK, CV_32FC1);
//    cv::Mat ff1;
//    frame_t1.convertTo(ff1, CV_32FC1);
//    cv::Mat ff2;
//    frame_t2.convertTo(ff2, CV_32FC1);
//
//    cv::triangulatePoints( frame_t1, frame_t2, pts_1, pts_2, pts_4d );
//
//    // 转换成非齐次坐标
//    for ( int i=0; i<pts_4d.cols; i++ )
//    {
//        cv::Mat x = pts_4d.col(i);
//        x /= x.at<float>(3,0); // 归一化
//        cv::Point3d p (
//                x.at<float>(0,0),
//                x.at<float>(1,0),
//                x.at<float>(2,0)
//        );
//        points.push_back( p );
//    }
//    int m=0;
//}


void triangulation (
        Relocation_Slam::Frame* f1,
        Relocation_Slam::Frame* f2,
        cv::Mat K,
        vector<pair<int,int>> matches,
        vector< cv::Point3f >& points )
{
    cv::Mat T1;
    cv::eigen2cv(f1->mPose.inverse().matrix(),T1);
    cv::Mat frame_t1 = (cv::Mat_<double> (3,4) <<
            T1.at<double>(0,0),T1.at<double>(0,1),T1.at<double>(0,2),T1.at<double>(0,3),
            T1.at<double>(1,0), T1.at<double>(1,1), T1.at<double>(1,2), T1.at<double>(1,3),
            T1.at<double>(2,0), T1.at<double>(2,1), T1.at<double>(2,2), T1.at<double>(2,3));

    cv::Mat T2;
    cv::eigen2cv(f2->mPose.inverse().matrix(),T2);
    cv::Mat frame_t2 = (cv::Mat_<double> (3,4) <<
            T2.at<double>(0,0),T2.at<double>(0,1),T2.at<double>(0,2),T2.at<double>(0,3),
            T2.at<double>(1,0), T2.at<double>(1,1), T2.at<double>(1,2), T2.at<double>(1,3),
            T2.at<double>(2,0), T2.at<double>(2,1), T2.at<double>(2,2), T2.at<double>(2,3));

    vector<cv::Point2f> pts_1, pts_2;

    for ( int i=0;i<matches.size();i++ )
    {
        // 将像素坐标转换至相机坐标
        pts_1.push_back ( pixel2cam( f1->mvKeyPoints[matches[i].first].pt, K) );
        pts_2.push_back ( pixel2cam( f2->mvKeyPoints[matches[i].second].pt, K) );
    }
    cv::Mat pts_4d;
    cv::triangulatePoints( frame_t1, frame_t2, pts_1, pts_2, pts_4d );

    // 转换成非齐次坐标
    for ( int i=0; i<pts_4d.cols; i++ )
    {
        cv::Mat x = pts_4d.col(i);
        x /= x.at<float>(3,0); // 归一化
        cv::Point3d p (
                x.at<float>(0,0),
                x.at<float>(1,0),
                x.at<float>(2,0)
        );
        points.push_back( p );
    }
}


void triangulation (
        Relocation_Slam::Frame f1,
        Relocation_Slam::Frame f2,
        cv::Mat K,
        vector<pair<int,int>> matches,
        vector< cv::Point3f >& points )
{
    cv::Mat T1;
    cv::eigen2cv(f1.mPose.inverse().matrix(),T1);
    cv::Mat frame_t1 = (cv::Mat_<double> (3,4) <<
                                               T1.at<double>(0,0),T1.at<double>(0,1),T1.at<double>(0,2),T1.at<double>(0,3),
            T1.at<double>(1,0), T1.at<double>(1,1), T1.at<double>(1,2), T1.at<double>(1,3),
            T1.at<double>(2,0), T1.at<double>(2,1), T1.at<double>(2,2), T1.at<double>(2,3));

    cv::Mat T2;
    cv::eigen2cv(f2.mPose.inverse().matrix(),T2);
    cv::Mat frame_t2 = (cv::Mat_<double> (3,4) <<
                                               T2.at<double>(0,0),T2.at<double>(0,1),T2.at<double>(0,2),T2.at<double>(0,3),
            T2.at<double>(1,0), T2.at<double>(1,1), T2.at<double>(1,2), T2.at<double>(1,3),
            T2.at<double>(2,0), T2.at<double>(2,1), T2.at<double>(2,2), T2.at<double>(2,3));

    vector<cv::Point2f> pts_1, pts_2;

    for ( int i=0;i<matches.size();i++ )
    {
        // 将像素坐标转换至相机坐标
        pts_1.push_back ( pixel2cam( f1.mvKeyPoints[matches[i].first].pt, K) );
        pts_2.push_back ( pixel2cam( f2.mvKeyPoints[matches[i].second].pt, K) );
    }

    cv::Mat pts_4d;
    cv::triangulatePoints( frame_t1, frame_t2, pts_1, pts_2, pts_4d );

    // 转换成非齐次坐标
    for ( int i=0; i<pts_4d.cols; i++ )
    {
        cv::Mat x = pts_4d.col(i);
        x /= x.at<float>(3,0); // 归一化
        cv::Point3d p (
                x.at<float>(0,0),
                x.at<float>(1,0),
                x.at<float>(2,0)
        );
        points.push_back( p );
    }
}

#endif //RELOCATION_SLAM_TRIANGLE_H

